#ros lib
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan

#commom lib
import math
import numpy as np
import time
import cv2
import os
from time import sleep
from yahboomcar_laser.common import *
import datetime

print ("improt done")
RAD2DEG = 180 / math.pi


class laserTracker(Node):
    def __init__(self,name):
        super().__init__(name)
        #create a sub
        self.sub_laser = self.create_subscription(LaserScan,"/scan",self.registerScan,1)
        #create a pub
        self.pub_vel = self.create_publisher(Twist,'/cmd_vel',1)
        
        #declareparam
        self.declare_parameter("linear",0.5)
        self.linear = self.get_parameter('linear').get_parameter_value().double_value
        self.declare_parameter("angular",1.0)
        self.angular = self.get_parameter('angular').get_parameter_value().double_value
        self.declare_parameter("LaserAngle",20.0)
        self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
        self.declare_parameter("ResponseDist",0.8)
        self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value
        self.declare_parameter("Switch",False)
        self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
        
        # 容差距离
        self.distance_tolerance = 0.15
     
        self.Joy_active = False
        self.ros_ctrl = SinglePID()
        self.priorityAngle = 60  # 40
        self.capturing = False
        self.aiming = False
        # 当前速度
        self.last_time = time.time()
        self.count_time = 0.0
        self.time_slice = 1.5
        self.photo_ready_time = 0.0

        # self.current_velocity = Twist()

        self.target_direction={
            "dist":0.0,
            "direction":0.0
        }

        self.current_dist = 0.0
        self.current_distID = 0.0

        
        self.lin_pid = SinglePID(2.0, 0.0, 2.0)
        self.ang_pid = SinglePID(3.0, 0.0, 5.0)

        # 调整PID参数以适应新的控制策略
        # 距离控制PID - 只控制角速度来调节距离
        self.distance_pid = SinglePID(1.5, 0.0, 0.8)
        # 角度控制PID - 保持朝向目标
        self.angle_pid = SinglePID(1.5, 0.0, 0.9)
        
        # 恒定的切向速度
        self.tangential_speed = 0.25

        self.frame_count=0
        self.cap = cv2.VideoCapture(0)
        if not self.cap.isOpened():
            print("无法打开摄像头")
            raise Exception("无法打开摄像头")

        self.distance_clamp_min=0.2
        self.distance_clamp_max=3.0

        # 摄像机模式
        # 创建带时间戳的子目录
        timestamp_dir = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
        self.save_dir = os.path.join(
            "/root/yahboomcar_ros2_ws/yahboomcar_ws/src/yahboomcar_laser/yahboomcar_laser/image",
            timestamp_dir
        )
        os.makedirs(self.save_dir, exist_ok=True)


        # 尝试设置为摄像头支持的最大分辨率（如 1920x1080）
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)

        width = int(self.cap.get(cv2.CAP_PROP_FRAME_WIDTH))
        height = int(self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
        print(f"当前摄像头分辨率: {width}x{height}")

        self.timer = self.create_timer(0.05,self.on_timer)
        # self.timer_capture = self.create_timer(0.1,self.capture_camera)

    def capture_camera(self):
        print("保存照片")
        ret, frame = self.cap.read()
        if not ret:
            print("读取帧失败")
            return
        self.frame_count+=1
        timestamp = time.strftime("%Y%m%d_%H%M%S", time.localtime())
        filename = os.path.join(self.save_dir, f"{timestamp}_{self.frame_count:04d}.jpg")
        success = cv2.imwrite(filename, frame)
        if success:
            print(f"成功保存图像: {filename}")
        else:
            print(f"图像保存失败: {filename}")
        self.capturing = False

    def on_timer(self):
        self.Switch = self.get_parameter('Switch').get_parameter_value().bool_value
        self.angular = self.get_parameter('angular').get_parameter_value().double_value
        self.linear = self.get_parameter('linear').get_parameter_value().double_value
        self.LaserAngle = self.get_parameter('LaserAngle').get_parameter_value().double_value
        self.ResponseDist = self.get_parameter('ResponseDist').get_parameter_value().double_value

        delta_time = time.time() - self.last_time
        self.last_time = time.time()
        

        if self.count_time > self.time_slice:
            self.capturing = True
            self.aiming = True
            self.count_time = 0.0

        if self.capturing:
            if self.aiming:
                self.aim_target()
            else:
                if self.photo_ready_time >= 0.5:
                    self.pub_vel.publish(Twist())
                    self.photo_ready_time = 0.0
                    # 等待小车停下并摄像机对焦
                    self.capture_camera()
                    self.capturing = False
                else:
                    self.pub_vel.publish(Twist())
                    self.photo_ready_time += delta_time

            return
        
        self.count_time += delta_time
        self.run_circle()

    def aim_target(self):
        velocity = Twist()

        minDist = self.current_dist
        minDistID = self.current_distID

        # 距离误差处理 - 如果距离接近目标距离，则视为已到达
        if abs(minDist - self.ResponseDist) < 0.1: 
            minDist = self.ResponseDist

        # 线性速度控制：向目标距离移动
        velocity.linear.x = -self.lin_pid.pid_compute(self.ResponseDist, minDist)
        
        # 角速度控制：调整朝向
        ang_pid_compute = self.ang_pid.pid_compute((180 - abs(minDistID)) / 72, 0)

        if minDistID > 0: 
            velocity.angular.z = -ang_pid_compute
        else: 
            velocity.angular.z = ang_pid_compute
        
        # 如果角度误差很小，停止旋转
        if ang_pid_compute < 0.02: 
            velocity.angular.z = 0.0
        
        # 如果角度和距离都达到目标，停止瞄准
        if ang_pid_compute < 0.02 and abs(velocity.linear.x) < 0.08: 
            self.aiming = False  # 修正：变量名从 aimming 改为 aiming
            time.sleep(0.5)
            return
        print("对齐模式")
        print(f"运动指令参数 linear.x:{velocity.linear.x:.3f}, linear.y:{velocity.linear.y:.3f}, angular.z:{velocity.angular.z:.3f}")
        self.pub_vel.publish(velocity)

    def run_circle(self):
        velocity = Twist()

        # 计算转弯半径 R
        base_angular_velocity = 0.0
        base_angular_velocity = -self.tangential_speed/(self.ResponseDist) # 将角度转换为弧度
        velocity.linear.y = self.tangential_speed  # 恒定切线速度
        velocity.angular.z = base_angular_velocity
        print("旋转模式")
        print(f"运动指令参数 linear.x:{velocity.linear.x:.3f}, linear.y:{velocity.linear.y:.3f}, angular.z:{velocity.angular.z:.3f}")
        self.pub_vel.publish(velocity)


    def registerScan(self, scan_data):
        if not isinstance(scan_data, LaserScan): return
        ranges = np.array(scan_data.ranges)
        offset = 0.5
        
        velocity = Twist()
        frontDistList = []
        frontDistIDList = []
        minDistList = []
        minDistIDList = []

        
        for i in range(len(ranges)):
            angle = (scan_data.angle_min + scan_data.angle_increment * i) * RAD2DEG
            if abs(angle) > (180 - self.priorityAngle):
            	if ranges[i] < (self.ResponseDist + offset):
            		frontDistList.append(ranges[i])
            		frontDistIDList.append(angle)
            elif (180 - self.LaserAngle) < angle < (180 - self.priorityAngle):
            	minDistList.append(ranges[i])
            	minDistIDList.append(angle)
            elif (self.priorityAngle - 180) < angle < (self.LaserAngle - 180):
            	minDistList.append(ranges[i])
            	minDistIDList.append(angle)
        if len(frontDistIDList) != 0:
        	minDist = min(frontDistList)
        	minDistID = frontDistIDList[frontDistList.index(minDist)]
        elif minDistList:
        	minDist = min(minDistList)
        	minDistID = minDistIDList[minDistList.index(minDist)]
        else:
            minDist = self.ResponseDist
            minDistID=180

        print(f"方位角{minDistID}")


        # 距离误差处理
        if abs(minDist - self.ResponseDist) < 0.06: 
            minDist = self.ResponseDist

        self.current_dist=minDist
        self.current_distID=minDistID


    def shutdown_cleanup(self):
       """退出时的清理工作"""
       print("正在安全退出...")
       
       # 发送停止指令
       for _ in range(3):
           self.pub_vel.publish(Twist())
           time.sleep(0.05)
       print("已发送停止指令")

def main():
    rclpy.init()
    laser_tracker = laserTracker("laser_Tracker_a1")
    print ("start it")
    try:
        rclpy.spin(laser_tracker)
    except KeyboardInterrupt:
        pass
    finally:
        print("重置小车状态")
        laser_tracker.pub_vel.publish(Twist())
        laser_tracker.shutdown_cleanup()
        laser_tracker.destroy_node()
        rclpy.shutdown()

import time

if __name__ == '__main__':
    main()